/***************************************************
This is a PID program based off of the following
Created by Justin Gerein for usage by the University of Saskatchewan Space Design
and any team members for personal usage

PLaces for improvement
- Consider putting in a limit for the integrator function so it cannot ramp 
up the value so much it is ridiculous..?


 ****************************************************/
 

#include "Arduino.h"

#include "PID.h"

/******************************************
Constructor
*******************************************/
PID::PID()
{  
}


/******************************************
Caluclator
*******************************************/
float PID::Calculate(float input, float target, float scaler, float Kd, float Ki, float Kp)
{

  //Record current time and all nexessary time changes
  currentTime = millis();
  dTime = (currentTime-previousTime);
  
  //Calcute all nexessary changes/variables
  error = target - input;
  derivative = (error-lastError)/dTime;
  integral += error*dTime/1000;
  
  //Calculate the total output change due to the 
  output = error * Kp + integral * Ki + derivative * Kd;
  
  /* The output is scaled depending on the operation in which the PID is being
  used. By scaling it, the output can be translated into a magnitude that is
  meaningful to the hardware that will be receiving this output
  */
  output *= scaler;
  
  
  //Store some variables for next time
  lastError = error;
  previousTime = currentTime;
  
  //give the alteration of the value back to the main program
  return output;
  
}

